
#include <cstdio>
#include <BSliderRestraint>
#include "member_BSliderRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BSliderRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BSliderRestraint(this)
#define member_release()		delete (member_BSliderRestraint*)_ptr

BSliderRestraint::BSliderRestraint()
{
	member_allocate();
}
BSliderRestraint::BSliderRestraint(const BString& name) : BRestraint(name)
{
	member_allocate();
}
BSliderRestraint::BSliderRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BSliderRestraint::BSliderRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BSliderRestraint::BSliderRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BSliderRestraint::~BSliderRestraint()
{
	member_release();
}

void BSliderRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);

}
BRestraint* BSliderRestraint::clone() const
{
	BSliderRestraint* restraint = new BSliderRestraint();
	restraint->copy(this);
	return restraint;
}

void BSliderRestraint::setStripe(BLineStripe* stripe)
{
	if (member.stripe != stripe)
	{
		if (member.stripe)
			member.disconnect(member.stripe);
		member.stripe = stripe;
		if (member.stripe)
			member.connect(member.stripe, Signal_Flushed, &member_BSliderRestraint::slotLineStripeFlushed);
		member.linearLimit.set(0, member.stripe->length());
		this->emit(Signal_Changed);
	}
}
BLineStripe* BSliderRestraint::stripe() const
{
	return member.stripe;
}

void BSliderRestraint::setCurve(BSplineCurve* curve)
{
	if (member.curve != curve)
	{
		if (member.curve)
			member.disconnect(member.curve);
		member.curve = curve;
		if (member.curve)
			member.connect(member.curve, Signal_Flushed, &member_BSliderRestraint::slotSplineCurveFlushed);
		member.linearLimit.set(0, member.curve->length());
		this->emit(Signal_Changed);
	}
}
BSplineCurve* BSliderRestraint::curve() const
{
	return member.curve;
}

bool BSliderRestraint::setAxisA(BReal ax, BReal ay, BReal az)
{
	return setAxisA(BVector(ax, ay, az));
}
bool BSliderRestraint::setAxisA(const BVector& axisA)
{
	if (member.axisA != axisA)
	{
		member.axisA = axisA;
		this->emit(Signal_Changed);
		return true;
	}
	return false;
}
const BVector& BSliderRestraint::axisA() const
{
	return member.axisA;
}

bool BSliderRestraint::setAxisB(BReal ax, BReal ay, BReal az)
{
	return setAxisB(BVector(ax, ay, az));
}
bool BSliderRestraint::setAxisB(const BVector& axisB)
{
	if (member.axisB != axisB)
	{
		member.axisB = axisB;
		this->emit(Signal_Changed);
		return true;
	}
	return false;
}
const BVector& BSliderRestraint::axisB() const
{
	return member.axisB;
}

void BSliderRestraint::setLinearLimit(BReal lower, BReal upper)
{
	setLinearLimit(BRealRange(lower, upper));
}
void BSliderRestraint::setLinearLimit(const BRealRange& linearLimit)
{
	if (member.linearLimit != linearLimit)
	{
		member.linearLimit = linearLimit;
		this->emit(Signal_Changed);
	}
}
const BRealRange& BSliderRestraint::linearLimit() const
{
	return member.linearLimit;
}

void BSliderRestraint::setAngularLimit(BReal lower, BReal upper)
{
	setAngularLimit(BRealRange(lower, upper));
}
void BSliderRestraint::setAngularLimit(const BRealRange& angularLimit)
{
	if (member.angularLimit != angularLimit)
	{
		member.angularLimit = angularLimit;
		this->emit(Signal_Changed);
	}
}
const BRealRange& BSliderRestraint::angularLimit() const
{
	return member.angularLimit;
}

void BSliderRestraint::setLinearMotorEnabled(bool motorEnabled)
{
	if (member.linearMotorEnabled != motorEnabled)
	{
		member.linearMotorEnabled = motorEnabled;
		if (member.linearMotorReached)
		{
			member.linearMotorReached = false;
			this->emit(Signal_LinearMotorReached, false);
		}
		this->emit(Signal_LinearMotorEnabled, motorEnabled);
	}
}
bool BSliderRestraint::linearMotorEnabled() const
{
	return member.linearMotorEnabled;
}

void BSliderRestraint::setLinearMotorTarget(BReal motorTarget)
{
	if (member.linearMotorTarget != motorTarget)
	{
		member.linearMotorTarget = motorTarget;
		if (member.linearMotorReached)
		{
			member.linearMotorReached = false;
			this->emit(Signal_LinearMotorReached, false);
		}
		this->emit(Signal_LinearMotorChanged);
	}
}
BReal BSliderRestraint::linearMotorTarget() const
{
	return member.linearMotorTarget;
}

void BSliderRestraint::setLinearMotorVelocity(BReal motorVelocity)
{
	if (member.linearMotorVelocity != motorVelocity)
	{
		member.linearMotorVelocity = motorVelocity;
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BSliderRestraint::linearMotorVelocity() const
{
	return member.linearMotorVelocity;
}

void BSliderRestraint::setLinearMotorMaxForce(BReal motorMaxForce)
{
	if (member.linearMotorMaxForce != motorMaxForce)
	{
		member.linearMotorMaxForce = motorMaxForce;
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BSliderRestraint::linearMotorMaxForce() const
{
	return member.linearMotorMaxForce;
}

void BSliderRestraint::setAngularMotorEnabled(bool motorEnabled)
{
	if (member.angularMotorEnabled != motorEnabled)
	{
		member.angularMotorEnabled = motorEnabled;
		if (member.angularMotorReached)
		{
			member.angularMotorReached = false;
			this->emit(Signal_AngularMotorReached, false);
		}
		this->emit(Signal_AngularMotorEnabled, motorEnabled);
	}
}
bool BSliderRestraint::angularMotorEnabled() const
{
	return member.angularMotorEnabled;
}

void BSliderRestraint::setAngularMotorTarget(BReal motorTarget)
{
	if (member.angularMotorTarget != motorTarget)
	{
		member.angularMotorTarget = motorTarget;
		if (member.angularMotorReached)
		{
			member.angularMotorReached = false;
			this->emit(Signal_AngularMotorReached, false);
		}
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BSliderRestraint::angularMotorTarget() const
{
	return member.angularMotorTarget;
}

void BSliderRestraint::setAngularMotorVelocity(BReal motorVelocity)
{
	if (member.angularMotorVelocity != motorVelocity)
	{
		member.angularMotorVelocity = motorVelocity;
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BSliderRestraint::angularMotorVelocity() const
{
	return member.angularMotorVelocity;
}

void BSliderRestraint::setAngularMotorMaxForce(BReal motorMaxForce)
{
	if (member.angularMotorMaxForce != motorMaxForce)
	{
		member.angularMotorMaxForce = motorMaxForce;
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BSliderRestraint::angularMotorMaxForce() const
{
	return member.angularMotorMaxForce;
}

BReal BSliderRestraint::linearPos() const
{
	return member.linearPos;
}
bool BSliderRestraint::linearMotorReached() const
{
	return member.linearMotorReached;
}

BReal BSliderRestraint::angularPos() const
{
	return member.angularPos;
}
const BVector& BSliderRestraint::horAxis() const
{
	return member.horAxis;
}
const BVector& BSliderRestraint::verAxis() const
{
	return member.verAxis;
}
const BVector& BSliderRestraint::dirAxis() const
{
	return member.dirAxis;
}
const BVector& BSliderRestraint::swingAxis() const
{
	return member.swingAxis;
}
bool BSliderRestraint::angularMotorReached() const
{
	return member.angularMotorReached;
}

bool BSliderRestraint::query(const BString& text, BValue& value) const
{
	return BRestraint::query(text, value);
}

int BSliderRestraint::solverSize()
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector pointA = matrixA * restraint_member(this)->pivotA;
	BVector pointB = matrixB * restraint_member(this)->pivotB;

	member.linearPos = 0;
	if (member.stripe)
	{
		const BVectorArray* vertices = member.stripe->vertices();
		if (vertices && vertices->size() > 2)
		{
			member.linearLimit.set(0, member.stripe->length());
			BMatrix lineMatrix = member.stripe->matrix();
			BVector pivotA;
			BVector axisA;
			member.linearPos = member.stripe->closest((lineMatrix * matrixA).inverse() * pointB, pivotA, axisA);
			this->setPivotA(lineMatrix * pivotA);
			this->setAxisA(lineMatrix.mult3(axisA));
		}
	}
	else if (member.curve)
	{
		const BVectorArray* vertices = member.curve->vertices();
		if (vertices && vertices->size() > 2)
		{
			member.linearLimit.set(0, member.curve->length());
			BMatrix lineMatrix = member.curve->matrix();
			BVector pivotA;
			BVector axisA;
			member.linearPos = member.curve->closest((lineMatrix * matrixA).inverse() * pointB, pivotA, axisA);
			this->setPivotA(lineMatrix * pivotA);
			this->setAxisA(lineMatrix.mult3(axisA));
		}
	}
	member.linearCaculate();

	if (member.axisA.length2() < REAL_EPSILON || member.axisB.length2() < REAL_EPSILON)
		return 0;

	member.angularCaculate();

	int num = 4;
	if (member.linearLimited || member.linearMotorEnabled)
	{
		num++;
	}
	if (member.angularLimited || member.angularMotorEnabled)
	{
		num++;
	}
	return num;
}
void BSliderRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	const BVector& linearVelocityA = body_member(bodyA)->linearVelocity;
	const BVector& linearVelocityB = body_member(bodyB)->linearVelocity;

	BVector pointA = matrixA * restraint_member(this)->pivotA;
	BVector pointB = matrixB * restraint_member(this)->pivotB;
	BVector offset = pointB - pointA;

	BReal invMassA = solverItems[0].invMassA;
	BReal invMassB = solverItems[0].invMassB;

	BReal imSum = invMassA + invMassB;
	BReal factA = imSum > REAL_EPSILON ? invMassB / imSum : BReal(0.5);
	BReal factB = 1 - factA;

	BVector axisA = matrixA.mult3(member.axisA);
	BVector axisB = matrixB.mult3(member.axisB);

	BVector axis = axisA * factA + axisB * factB;
	axis.normalize();

	{
		BVector p, q;
		PlaneSpace(axis, p, q);

		BVector u = axisA.cross(axisB);

		solverItems[0].torqueAxisA = p;
		solverItems[0].torqueAxisB = -p;
		solverItems[0].velocity = u.dot(p) / stepTime;

		solverItems[1].torqueAxisA = q;
		solverItems[1].torqueAxisB = -q;
		solverItems[1].velocity = u.dot(q) / stepTime;
	}

	BVector vectorA = pointA - matrixA.position();
	BVector projA = axis * vectorA.dot(axis);
	BVector orthoA = vectorA - projA;

	BVector pob = pointB;
	BVector vectorB = pointB - matrixB.position();
	BVector projB = axis * vectorB.dot(axis);
	BVector orthoB = vectorB - projB;

	BVector totalDist = projA + axis * member.linearDiff - projB;

	vectorA = orthoA + totalDist * factA;
	vectorB = orthoB - totalDist * factB;
	BVector p = orthoB * factA + orthoA * factB;
	BReal len2 = p.length2();
	if (len2 > REAL_EPSILON)
	{
		p /= sqrt(len2);
	}
	else
	{
		BVector pa, qa;
		PlaneSpace(member.axisA, pa, qa);
		p = matrixA.mult3(pa);
	}
	BVector q = axis.cross(p);
	solverItems[2].torqueAxisA = vectorA.cross(p);
	solverItems[2].torqueAxisB = -vectorB.cross(p);
	solverItems[2].contactNormalA = p;
	solverItems[2].contactNormalB = -p;
	solverItems[2].velocity = p.dot(offset) / stepTime;

	BVector tmpA = vectorA.cross(q);
	BVector tmpB = vectorB.cross(q);
	if (member.angularLimited)
	{
		tmpB *= factB;
		tmpA *= factA;
	}
	solverItems[3].torqueAxisA = tmpA;
	solverItems[3].torqueAxisB = -tmpB;
	solverItems[3].contactNormalA = q;
	solverItems[3].contactNormalB = -q;
	solverItems[3].velocity = q.dot(offset) / stepTime;

	int row = 3;
	if (member.linearLimited || member.linearMotorEnabled)
	{
		row++;
		solverItems[row].contactNormalA = axis;
		solverItems[row].contactNormalB = -axis;
		solverItems[row].torqueAxisA = vectorA.cross(axis);
		solverItems[row].torqueAxisB = -vectorB.cross(axis);

		solverItems[row].velocity = 0;
		solverItems[row].impulseLowerLimit = 0;
		solverItems[row].impulseUpperLimit = 0;
		if (member.linearMotorEnabled)
		{
			BReal factor = member.linearMotorFactor(stepTime);
			solverItems[row].velocity -= factor * member.linearMotorVelocity;
			solverItems[row].impulseLowerLimit = -member.linearMotorMaxForce * stepTime;
			solverItems[row].impulseUpperLimit = member.linearMotorMaxForce * stepTime;
		}
		if (member.linearLimited)
		{
			solverItems[row].velocity += member.linearDiff / stepTime * 0.5;
			solverItems[row].impulseLowerLimit = -REAL_MAX;
			solverItems[row].impulseUpperLimit = REAL_MAX;
			if (member.linearLimit.min() < member.linearLimit.max())
			{
				if (member.linearDiff > 0)
				{
					solverItems[row].impulseLowerLimit = 0;
				}
				else
				{
					solverItems[row].impulseUpperLimit = 0;
				}
			}
		}
	}

	if (member.angularLimited || member.angularMotorEnabled)
	{
		row++;
		solverItems[row].torqueAxisA = -axis;
		solverItems[row].torqueAxisB = axis;

		solverItems[row].velocity = 0;
		if (member.angularMotorEnabled && !bZero(member.angularMotorVelocity))
		{
			BReal factor = member.angularMotorFactor(stepTime);
			solverItems[row].velocity += factor * member.angularMotorVelocity;
			solverItems[row].impulseLowerLimit = -member.angularMotorMaxForce * stepTime;
			solverItems[row].impulseUpperLimit = member.angularMotorMaxForce * stepTime;
		}
		if (member.angularLimited)
		{
			solverItems[row].velocity += member.angularDiff / stepTime * 0.5;
			if (member.angularLimit.min() > member.angularLimit.max())
			{
				solverItems[row].impulseLowerLimit = -REAL_MAX;
				solverItems[row].impulseUpperLimit = REAL_MAX;
			}
			else if (member.angularDiff > 0)
			{
				solverItems[row].impulseLowerLimit = 0;
				solverItems[row].impulseUpperLimit = REAL_MAX;
			}
			else if (member.angularDiff < 0)
			{
				solverItems[row].impulseLowerLimit = -REAL_MAX;
				solverItems[row].impulseUpperLimit = 0;
			}
		}
	}
}
